#! /usr/bin/env python
'''
    该类是一个小车底盘运动学解析线程，该线程的目标就是通过不断从临界区获取指令
    之后根据指令的内容对底盘的三个轮子进行运动学解析。
    解析完成后将该消息发布出去。在硬件接口层接受之后，会将该信息传送给stm32
    为了更好的解析指令，需要一个类来将运动学指令解析成和接口对应的 速度-角度-角速度 的形式
    即有一个函数parseCmd(self,cmd)
    这里根据电机的转速大小要对于输出作限制
    需要在配置文件中说明
'''
import re
import sys
sys.path.append("/home/galm/ms_01/src/motion_parser/scripts/kinematicsParsers")
from threading import Thread
from time import sleep
from motion_parser.msg import car_cmd
from kinematicsParsers.tripleOmniWheels import TripleOmniWheelsParser
import rospy

#自定义类
class CarThread(Thread):

 def __init__(self,queue):
   Thread.__init__(self)
   self.queue = queue
   #获取话题名称  CarParserTopic: car_parser_cmd
   topicName = rospy.get_param("CarParserTopic","car_parser_cmd")
   #设置发布者publisher
   self.publisher = rospy.Publisher(topicName,car_cmd,queue_size=10)
   #生成解析器实例对象
   self.motionParser = TripleOmniWheelsParser()
   #下面获取配置文件中对于小车速度的配置信息
   '''
    CarLinearVel: 2.0
    CarRotationVel: 0.4
    CarMotorMaxSpeed: 100
   '''
   #小车线性移动的速度大小
   self.linearVel = float(rospy.get_param("CarLinearVel","2.0"))
   #小车做旋转运动时的角速度大小
   self.rotationVel = float(rospy.get_param("CarRotationVel","0.4"))
   #小车车轮的速度限制，之后传给硬件接口层的就是对应每个车轮的转速了
   self.speedLimit = float(rospy.get_param("CarMotorMaxSpeed","100"))
    

 def execute(self):
      while True:
         #时刻检查更新
         self.update()
         #检查更新后根据命令实时计算位置并且调整信息
         #由于目前仅仅取决于电机pid控制，因此只需要在信息更改之后发布一次指令即可，电机会根据该转速不断调整的
         #self.calculate()
         sleep(0.1)
    #   self.epilogue()

 def run(self):
    self.execute()


 '''
    由于这个线程作为守护线程，是从程序开始到程序终止一直运行的，因此只需要时刻检查更新即可
    上一个节点（manager）传来的信息是封装好的数组，即是[a,b]的形式，其中a代表小车的线速度，即前后方向的速度，b代表小车的旋转和左右平移（因为小车没办法或者不常需要左右平移同时旋转）
    a如果是1代表小车前进，为-1代表小车后退
    b如果是1代表小车右转（以顺时针为正方向），为-1代表左转。而为2代表向右平移，为-2代表向左平移

    明天的任务：
    在之后的计算中，应该重点注意小车的速度角度计算，根据不同的指令得到角度值之后，就方便进行处理了。
 '''
 def update(self):
     while(not self.queue.empty()):
         self.cmd = self.queue.get()
         #这里获取了cmd信息，随即对其进行解析，并调用解算函数对三个电机转速进行解算
         self.calculate()


 def commandParse(self,cmd):
     #命令解算，从传来的数组中解算出对应的角速度以及角度，返回一个包含角度，速度和角速度的数组
     linear = int(cmd[0])
     rotation = int(cmd[1])
     vel = self.linearVel
     velAngle = 0
     omega = 0
     #接下来根据这两个值分类讨论
     #如果旋转中只有1和-1，证明小车要具有角速度，此时直接乘以对应的速度即可  这样就计算出了对应的角速度大小
     if(rotation == -1 or rotation == 1 or rotation == 0):
         omega = rotation*self.rotationVel
     #接下来根据其它量计算线速度方向
     if(linear == 0 and (rotation == 0 or rotation == 1 or rotation == -1)):
         #如果小车没有前进，同时小车没有左右平移，证明线速度为0
         vel = 0
     elif(linear == 0 and(rotation == 2 or rotation == -2)):
         #如果小车没有前后移动，但是有左右平移，那么这样就可以计算出方向
         velAngle = 30+rotation*45
     elif((linear == 1 or linear == -1) and (rotation == 0 or rotation == 1 or rotation == -1)):
         #如果小车前后移动，但是并没有左右平移，这时只需要考虑前后的线速度
         velAngle = 120-linear*90
     elif((linear == 1 or linear == -1) and (rotation == 2 or rotation == -2)):
         #如果小车前后移动同时具有左右平移，那么就根据方向公式计算不同的角度即可
         velAngle = 120-linear*90+((-1)*linear*rotation/abs(linear*rotation))*45
     return [vel,velAngle,omega]
     

 def limit(self,res):
     for i in range(0,4):
         if(res[i] > self.speedLimit):
             temp = res[i]
             res[i] = self.speedLimit
             rospy.loginfo("[warning]:车轮转速超出限制:%s",str(temp))
     return res
         

 def package(self,res):
     msg = car_cmd()
     msg.wheelA = res[0]
     msg.wheelB = res[1]
     msg.wheelC = res[2]
     msg.wheelD = res[3]
     return msg

 def calculate(self):
     parseRes = self.commandParse(self.cmd)
     res = self.motionParser.wheelParser(parseRes[0],parseRes[1],parseRes[2])
     msg = self.package(self.limit(res))
     self.publisher.publish(msg)
     rospy.loginfo("车轮速度信息已更新")
     print(res)

 def epilogue(self):
   print("/////////////////////////")
   rospy.loginfo("CarParserThread ending")
   print("/////////////////////////")
     